#include <robot_chassis/CRobotVoltage.hpp>
#include <rclcpp/time.hpp>

using namespace std::chrono_literals;

CRobotVoltage::CRobotVoltage(const std::string& nodeName)
    : Node(nodeName), turnonRobot_(CTurnOnRobot::getInstance())
{
    // 声明参数
    this->declare_parameter<double>("battery_full_voltage", 12.6);
    this->declare_parameter<double>("battery_low_voltage", 10.0);

    fullVoltage_ = static_cast<float>(this->get_parameter("battery_full_voltage").as_double());
    lowVoltage_  = static_cast<float>(this->get_parameter("battery_low_voltage").as_double());

    // 创建发布者
    pub_ = this->create_publisher<sensor_msgs::msg::BatteryState>("chassis/battery_state", 10);

    // 创建定时器，1Hz (电池状态不需要高频率发布)
    timer_ = this->create_wall_timer(1000ms, std::bind(&CRobotVoltage::callback, this));

    RCLCPP_INFO(this->get_logger(), "电池状态节点已启动 - 满电电压: %.1fV, 低电压: %.1fV",
                fullVoltage_, lowVoltage_);
}

CRobotVoltage::~CRobotVoltage() {}

void CRobotVoltage::getVoltage()
{
    turnonRobot_.getRobotVoltage(voltageValue_);

    sensorBatteryData_.header.stamp    = this->now();
    sensorBatteryData_.header.frame_id = "battery_link";

    // 电压
    sensorBatteryData_.voltage = voltageValue_;

    // 电量百分比
    float percentage = (voltageValue_ - lowVoltage_) / (fullVoltage_ - lowVoltage_);
    percentage = std::max(0.0f, std::min(1.0f, percentage));
    sensorBatteryData_.percentage = percentage;

    // 电源状态
    sensorBatteryData_.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING;
    sensorBatteryData_.power_supply_health = sensor_msgs::msg::BatteryState::POWER_SUPPLY_HEALTH_GOOD;
    sensorBatteryData_.power_supply_technology = sensor_msgs::msg::BatteryState::POWER_SUPPLY_TECHNOLOGY_LIPO;
    sensorBatteryData_.present = true;
}

void CRobotVoltage::callback()
{
    getVoltage();
    pub_->publish(sensorBatteryData_);
}
